#include <QPainter>
#include <stdio.h>
#include "handlermap.h"



handlerMap::handlerMap(QObject *parent,ArClientBase *client) :
    QObject(parent),
    handleGetMapCB(this,&handlerMap::handleGetMap),
    handleGetMapNameCB(this,&handlerMap::handleGetMapName),
    arMap()
{

    handledClient = client;



    maplines.begin();
    maplines.begin();
    mapgoals.begin();
    mapfline.begin();
    mapfrect.begin();

    listOfMapObjects->begin();

    if(!handledClient->dataExists("getMap"));
    else{
        handledClient->addHandler("getMap", &handleGetMapCB);
        handledClient->addHandler("getMapName", &handleGetMapNameCB);
        handledClient->requestOnce("getMapName");
        handledClient->requestOnce("getMap");
    }


}




void handlerMap::handleGetMap(ArNetPacket *packet)
{

  char buffer[10000];
  int x;

  if (packet->getDataReadLength() == packet->getDataLength())
  {
    printf("Empty packet signifying end of map (for central forward)\n");
    return;
  }

  packet->bufToStr(buffer, sizeof(buffer));
  // if we got an end of line char instead of a line it means the map is over
  if (buffer[0] == '\0')
  {

      arMap.lock();
    arMap.parsingComplete();




    //Imprimir mapa here


    std::vector <ArPose> *puntos;
    std::vector <ArLineSegment> *lineas;


    puntos = arMap.getPoints();
    lineas = arMap.getLines();


    //QPen qpen(Qt::black);
    //QBrush qbrush(Qt::red);


    mappoints.resize(puntos->size());
    maplines.resize(lineas->size());


    QPen qpen(Qt::black);
    QBrush qbrush(Qt::black);

    qpen.setWidth(10);


    ArMapObjectsInterface * objsInterface = arMap.getInactiveObjects();
        //exit(1);
    listOfMapObjects = objsInterface->getMapObjects();


    printf("listOfMapObjects size: %d\n", listOfMapObjects->size());
    ArMapObject* obj = NULL;
    for (std::list<ArMapObject *>::const_iterator it = listOfMapObjects->begin(); it != listOfMapObjects->end();  it++)
        {
          obj = (*it);
          if (obj == NULL) {
          printf("NO OBJECTS\n");
        break;
          }
          printf("Object name: %s of type: %s\n",obj->getName(),obj->getType());
        }


    for(x=0;x<puntos->size();x++){

        ArPose temp;
        temp = puntos->at(x);
        QGraphicsEllipseItem *point;


        point = new QGraphicsEllipseItem(temp.getX()-5,temp.getY()*(-1)-5,10,10,0,0);

        point->setPen(qpen);
        point->setBrush(qbrush);

        mappoints[x] = point;
        emit signal_paint_ellipse(mappoints[x]);
    }


    for(x=0;x<lineas->size();x++){

        ArLineSegment temp2;
        temp2 = lineas->at(x);


        QGraphicsLineItem *line;


        line = new QGraphicsLineItem(temp2.getX1(),temp2.getY1()*(-1),temp2.getX2(),temp2.getY2()*(-1),0,0);

        line->setPen(qpen);

        maplines[x] = line;
        emit signal_paint_line(maplines[x]);

    }

    arMap.unlock();

  }
  else
  {
    arMap.lock();
    if(!arMap.parseLine(buffer)) {
        printf("Nueva linea: %s\n",buffer);
        QString parser(buffer);
        QString pattern("((\\\"\\\")|(\\\"\\w+\\s?\\w*\\\")|(([A-Z])\\w+:)|(([A-Z])\\w+\\.?\\w+)|(-?\\d+\\.?\\d*))");
        QRegExp qreg(pattern);
        qreg.setPatternSyntax(QRegExp::RegExp);




        QStringList *lista = new QStringList();

        lista->begin();


         int pos = 0;
         while ( (pos = qreg.indexIn(parser, pos)) != -1 )
         {

               lista->append(qreg.cap(1));
               pos +=qreg.matchedLength();
         }

         if(lista->at(0).contains("Cairn:")){
             if(lista->at(1).contains("Goal")||lista->at(1).contains("GoalWithHeading")){
                 myGoalItem *goalItem = new myGoalItem(Qt::lightGray,QString(lista->at(7)));
                 goalItem->setX(lista->at(2).toDouble());
                 goalItem->setY(lista->at(3).toDouble()*(-1));

                 double th=lista->at(4).toDouble();

                 //while(th<0) th=abs(th)+90;

                 if(th>180)th = 640-th;
                 else th = 180 - th;

                 th = ((int)th+270) % 360;


                 goalItem->setRotation(th);
                 goalItem->setZValue(-90);


                 mapgoals.push_back(goalItem);
                 emit(signal_paint_goal(goalItem));
             }else if(lista->at(1).contains("Dock")){
                 myGoalItem *goalItem = new myGoalItem(Qt::yellow,QString(lista->at(7)));
                 goalItem->setX(lista->at(2).toDouble());
                 goalItem->setY(lista->at(3).toDouble()*(-1));

                 double th=lista->at(4).toDouble();

                 //while(th<0) th=abs(th)+90;

                 if(th>180)th = 640-th;
                 else th = 180 - th;

                 th = ((int)th+270) % 360;


                 goalItem->setRotation(th);
                 goalItem->setZValue(-90);


                 mapgoals.push_back(goalItem);
                 emit(signal_paint_goal(goalItem));

             }else if(lista->at(1).contains("ForbiddenLine")){
                 QGraphicsLineItem *lineItem = new QGraphicsLineItem(lista->at(8).toDouble(),lista->at(9).toDouble()*(-1),lista->at(10).toDouble(),lista->at(11).toDouble()*(-1),0,0);


                 QPen qpen(QColor("orange"));
                 QBrush qbrush(Qt::black);

                 qpen.setWidth(10);


                 lineItem->setPen(qpen);
                 lineItem->setZValue(-100);


                 mapfline.push_back(lineItem);
                 emit(signal_paint_line(lineItem));

             }else if(lista->at(1).contains("ForbiddenArea")){
                 QGraphicsRectItem *rectItem = new QGraphicsRectItem(lista->at(8).toDouble(),lista->at(11).toDouble()*(-1),abs(lista->at(8).toDouble()-lista->at(10).toDouble()),abs(lista->at(9).toDouble()-lista->at(11).toDouble()),0,0);


                 QPen qpen(QColor("black"));
                 QBrush qbrush(QColor("orange"));

                 qpen.setWidth(10);


                 rectItem->setPen(qpen);
                 rectItem->setBrush(qbrush);
                 rectItem->setZValue(-100);

                 mapfrect.push_back(rectItem);
                 emit(signal_paint_rect(rectItem));

             }else if(lista->at(1).contains("RobotHome")){

                 if(lista->count()>8){
                 QGraphicsRectItem *rectItem = new QGraphicsRectItem(lista->at(8).toDouble(),lista->at(11).toDouble()*(-1),abs(lista->at(8).toDouble()-lista->at(10).toDouble()),abs(lista->at(9).toDouble()-lista->at(11).toDouble()),0,0);


                 QPen qpen(QColor("black"));
                 QBrush qbrush(QColor("green"));

                 qpen.setWidth(10);


                 rectItem->setPen(qpen);
                 rectItem->setBrush(qbrush);
                 rectItem->setZValue(-100);

                 mapfrect.push_back(rectItem);
                 emit(signal_paint_rect(rectItem));
                 }else{


                     myGoalItem *goalItem = new myGoalItem(Qt::green,QString(lista->at(7)));
                     goalItem->setX(lista->at(2).toDouble());
                     goalItem->setY(lista->at(3).toDouble()*(-1));

                     double th=lista->at(4).toDouble();

                     //while(th<0) th=abs(th)+90;

                     if(th>180)th = 640-th;
                     else th = 180 - th;

                     th = ((int)th+270) % 360;


                     goalItem->setRotation(th);
                     goalItem->setZValue(-90);


                     mapgoals.push_back(goalItem);
                     emit(signal_paint_goal(goalItem));


                 }
            }



         }

        //(\"\")|(\"\w+\s?\w*\")|(([A-Z])\w+:)|(([A-Z])\w+\.?\w+)|(-?\d+\.?\d*)
        // Enteros: -?\d*
        // Reales: -?\d*\.\d*
        // Cadenas: "(\w*\s*)+"
        // Cadenas sin comillas: \w+
        // reg.exp: (-?\d*|

    }
    arMap.unlock();


    //if (!strncmp(buffer,"Cairn:",6))         printf("Nueva linea: %s\n",buffer);
  }

}

void handlerMap::handleGetMapName(ArNetPacket *packet)
{
  char buffer[512];

  packet->bufToStr(buffer, sizeof(buffer));
  printf("MapFile: %s\n", buffer);
}

